#include "./tube.h"
#include "../device/gpio.h"
#include "./spi_flash.h"
#include "../rtos/rtos.h"
#include "../utils/prog.h"
#include "./sensor.h"
#include "./ctrl.h"

namespace driver {

using namespace device;

struct Info {
    uint16_t speed;

    void reset() {
        speed = 200;
    }
};

static store_t<TubeAddr, Info> info;

using PWM = PB<9>; // TIM11_CH1 <=> AF3
using DIR1 = PD<14>;
using DIR2 = PD<15>;
using COUNTER = PE<9>; // TIM1_CH1 <=> AF1

using namespace utils;

Tube::Tube() {

    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN +
        RCC_AHB1ENR_GPIODEN +
        RCC_AHB1ENR_GPIOEEN;
    RCC->APB2ENR |= RCC_APB2ENR_TIM11EN + 
        RCC_APB2ENR_TIM1EN;
    
    ConfigAf<3, PWM>();
    ConfigOutput<DIR1, DIR2>();
    ConfigAf<1, COUNTER>();

	TIM11->CR1 = 0x00;
	TIM11->CR2 = 0x00;
	TIM11->PSC = 168 - 1;
	TIM11->ARR = 100 - 1;
	TIM11->CR1 = 100;
	TIM11->CCMR1 = 0x0060;
	TIM11->CCER = 0x0001;
	TIM11->CR1 = 0x01;
	TIM11->EGR = 0x01;

	TIM1->CR1 = 0x00;
	TIM1->CR2 = 0x00;
	TIM1->SMCR = 0x57;
	TIM1->CCMR1 = 0xF1;
	TIM1->CCER &= ~0x0A;
	TIM1->CR1 = 0x01;

	set_dir(dir_t::Brake);
	set_param(0.03f, 0.03f, 0.01f);

	info.load();
}

error_t Tube::draw(uint16_t speed, uint16_t steps) {
	error_t err;
	uint16_t position = 0;

	init_ctrl(speed);
	reset_count();
	set_dir(dir_t::Down);
	motor_run();
	ctrl::compr_on();
	for (;;) {
		osDelay(20);
		position = get_count();
		exec(position);
		err = is_ok();
		if (err) {
			ctrl::compr_off();
			break;
		}
		if (sensor->is_compr()) {
			break;
		}
		if (position >= steps) {
			ctrl::compr_off();
			err = ec::DrawFail;
			break;
		}
	}
	set_dir(dir_t::Brake);
	init_ctrl(0);
	return err;
}

error_t Tube::draw(uint16_t steps) {
	return draw(info->speed, steps);
}

error_t Tube::down(uint16_t speed, uint16_t steps) {
	error_t err;
	uint16_t position = 0;

	init_ctrl(speed);
	reset_count();
	set_dir(dir_t::Down);
	motor_run();
	for (; position < steps;) {
		osDelay(20);
		position = get_count();
		exec(position);
		err = is_ok();
		if (err) {
			break;
		}
	}
	set_dir(dir_t::Brake);
	init_ctrl(0);
	return err;
}

error_t Tube::down(uint16_t steps) {
	return down(info->speed, steps);
}

error_t Tube::up(uint16_t speed, uint16_t steps) {
	error_t err;
	uint16_t position = 0;
	bool top_flag = (steps == 0);

	if (sensor->is_tube_top()) {
		return err;
	}

	reset_count();
	init_ctrl(speed);
	set_dir(dir_t::Up);
	motor_run();
	for (; top_flag || (position < steps);) {
		osDelay(20);
		position = get_count();
		exec(position);
		err = is_ok();
		if (err || sensor->is_tube_top()) {
			break;
		}
	}
	set_dir(dir_t::Brake);
	init_ctrl(0);
	return err;
}

error_t Tube::up(uint16_t steps) {
	return up(info->speed, steps);
}

error_t Tube::reset() {
	return up(info->speed, 0);
}

void Tube::set_target_speed(uint16_t speed) {
	info->speed = speed;
	info.save();
}

uint16_t Tube::get_target_speed() {
	return info->speed;
}

uint16_t Tube::get_count() {
    return TIM1->CNT;
}

void Tube::reset_count() {
    TIM1->CNT = 0;
}

void Tube::set_voltage(uint8_t val) {
	TIM11->CCR1 = val;
}

void Tube::set_dir(dir_t dir) {
	switch (dir) {
		case dir_t::Down:
			DIR1::Clr();
			DIR2::Set();
		break;

		case dir_t::Up:
			DIR1::Set();
			DIR2::Clr();
		break;
		
		case dir_t::Brake:
			DIR1::Clr();
			DIR2::Clr();
			set_voltage(0);
		break;
	}
}

void Tube::test(dir_t dir, uint8_t sp) {
	set_dir(dir);
	set_voltage(sp);
}

}
